#ifndef COMPOSITION__LISTENER_COMPONENT_HPP_
#define COMPOSITION__LISTENER_COMPONENT_HPP_

// #include "composition/visibility_control.h"
#include "libbase/common.h"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "base_interfaces/msg/mat_with_time.hpp"
#include "cv_bridge/cv_bridge.hpp"
#include "ros_usbcapture/UsbCaptureSystem.h"
#include "sensor_msgs/msg/image.hpp"
#include "sensor_msgs/msg/camera_info.hpp"
#include <cv_bridge/cv_bridge.hpp>

namespace wmj
{

    class Image_get_test : public rclcpp::Node
    {
    public:
        // COMPOSITION_PUBLIC
        explicit Image_get_test(const rclcpp::NodeOptions &options) : Node("Image_get_test", options)
        {
            RCLCPP_INFO(rclcpp::get_logger("INFO"), "image_component get is working ");
            //创建订阅者

            rmw_qos_profile_t qos_t;
            qos_t.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
            qos_t.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST;
            qos_t.depth = 5;
            qos_t.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
            qos_t.deadline = RMW_QOS_DEADLINE_DEFAULT;
            qos_t.lifespan = RMW_QOS_LIFESPAN_DEFAULT;
            qos_t.liveliness = RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT;
            qos_t.liveliness_lease_duration = RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT;
            qos_t.avoid_ros_namespace_conventions = false;

            rclcpp::QoSInitialization qosInitialization = rclcpp::QoSInitialization::from_rmw(qos_t);
            rclcpp::QoS qos = rclcpp::QoS(qosInitialization, qos_t);

            sensorImgSub = this->create_subscription<sensor_msgs::msg::Image>(
                                "image_raw", qos, std::bind(&Image_get_test::SensorImageCallBack, this ,std::placeholders::_1));
            rightSensorImgSub = this->create_subscription<sensor_msgs::msg::Image>(
                                "right_image_raw", qos, std::bind(&Image_get_test::RightSensorImageCallBack, this ,std::placeholders::_1));
            leftSensorImgSub = this->create_subscription<sensor_msgs::msg::Image>(
                                "left_image_raw", qos, std::bind(&Image_get_test::LeftSensorImageCallBack, this ,std::placeholders::_1));
                                
            // bool use_sensor_data_qos = this->declare_parameter("use_sensor_data_qos", true);     // true时无法rqt无法显示
            // auto qos = use_sensor_data_qos ? rmw_qos_profile_sensor_data : rmw_qos_profile_default;                    
            // cameraSub = image_transport::create_camera_subscription(this, "image_raw", transCallback(), "trans", qos);
        }

    private:
        //声明回调函数
        void call_back(const base_interfaces::msg::MatWithTime::SharedPtr msg)
        {
            // RCLCPP_INFO(rclcpp::get_logger("INFO"), "get image msg");
            //利用 GetImg 获取图像，传入类型为 sensor_msgs::msg::Image
            cv::Mat image = GetImg(msg->image);

            //以下代码均为测试用：
            
            //获取地址
            // std::cout << "addr: 0x" << std::hex << &img << std::endl;

            // 显示图像
            cv::imshow("image",image);
            cv::waitKey(1);
            RCLCPP_INFO(rclcpp::get_logger("INFO"), "Sensor_Image Get!!!!!!!");

            // 输出图像地址
            // std::cout << "addr: " << std::hex << &(msg->image) << std::endl;
            
            //获取当前时间戳
            // double t = wmj::now();

            //计算帧率
            // all_time += t - msg->true_time_stamp;
            // if (all_time > 1)
            // {
            //     std::cout << "speed" << count << std::endl;
            //     all_time = 0;
            //     count =0;
            // }
            // else
            //     count++;

            
            //计算时间差
            // std::cout <<"hey:"<< t - msg->true_time_stamp  <<std::endl;
        }

        void SensorImageCallBack(const sensor_msgs::msg::Image::SharedPtr msg)
        {
            // printf("发送后： %p\n", msg.get());
            auto stamp = this->get_clock()->now();
            cv::Mat image = GetImg(*msg);
            
            builtin_interfaces::msg::Time t= stamp;
            double a;
            double b;
            a = t.sec + t.nanosec * 1e-9;
            b = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9;
            double c = a-b;

            RCLCPP_INFO(rclcpp::get_logger("INFO"), "Time: %lf",c);
            // 显示图像
            cv::imshow("sensor_image",image); 
            cv::waitKey(1);
        }
        void RightSensorImageCallBack(const sensor_msgs::msg::Image::SharedPtr msg)
        {
            // printf("发送后： %p\n", msg.get());
            auto stamp = this->get_clock()->now();
            cv::Mat image = GetImg(*msg);
            
            builtin_interfaces::msg::Time t= stamp;
            double a;
            double b;
            a = t.sec + t.nanosec * 1e-9;
            b = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9;
            double c = a-b;

            RCLCPP_INFO(rclcpp::get_logger("INFO"), "Time: %lf",c);
            // 显示图像
            cv::imshow(msg->header.frame_id,image); 
            cv::waitKey(1);
        }

        void LeftSensorImageCallBack(const sensor_msgs::msg::Image::SharedPtr msg)
        {
            // printf("发送后： %p\n", msg.get());
            auto stamp = this->get_clock()->now();
            cv::Mat image = GetImg(*msg);
            
            builtin_interfaces::msg::Time t= stamp;
            double a;
            double b;
            a = t.sec + t.nanosec * 1e-9;
            b = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9;
            double c = a-b;

            RCLCPP_INFO(rclcpp::get_logger("INFO"), "Time: %lf",c);
            // 显示图像
            cv::imshow(msg->header.frame_id,image); 
            cv::waitKey(1);
        }


        cv::Mat GetImg(sensor_msgs::msg::Image img)
        {
            // 声明 cv::image 指针，内含cv::Mat
            cv_bridge::CvImageConstPtr cv_ptr;
            // toCvShare() 使用
            // sensor_msgs::msg::Image::ConstPtr img_ptr;

            // 获取图像，最终图像通过 cv_ptr->image 获取，为cv::Mat型
            try
            {
                // 把msg里的 Image 转换为 cv::mat类型 , 有两种方式 toCvCopy() 和 toCvShare() ,toCvShare() 虽然可以实现直接引用不用拷贝，
                // 但需要创建 sensors_msgs::ImageConst 指针，经测试二者差别不大，故用toCvCopy().后续还可改进.

                cv_ptr = cv_bridge::toCvCopy(img);

                // toCvShare() 方法
                // img_ptr = std::make_shared<const sensor_msgs::msg::Image>(msg->image);
                // cv_ptr = cv_bridge::toCvCopy(img_ptr);
            }
            catch (cv_bridge::Exception &e)
            {
                printf("cv_bridge exception:%s", e.what());
            }
            return (cv_ptr->image);
        }
        
        // size_t count_;

        //声明发布者
        rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sensorImgSub;
        rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr rightSensorImgSub;
        rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr leftSensorImgSub;
        
        //用于测试帧率
        // int num;
        // double all_time=0;
        // int count = 0;
    };

} // namespace composition


#include "rclcpp_components/register_node_macro.hpp"

RCLCPP_COMPONENTS_REGISTER_NODE(wmj::Image_get_test)

#endif // COMPOSITION__LISTENER_COMPONENT_HPP_